package com.googlecode.curvedmetromaps.core.geometry;

public class BandedMatrix {

	int						mSize, mNumLBands, mNumUBands;
	double[]				mDBand;
	double[][]				mLBand;
	double[][]				mUBand;

	private final double	ZERO_TOLERANCE	= 1e-08;

	BandedMatrix(int size, int lBands, int uBands) {
		assert size > 0 && lBands >= 0 && uBands >= 0 && lBands < size && uBands < size : "Invalid argument to BandedMatrix constructor\n";

		mSize = size;
		mNumLBands = lBands;
		mNumUBands = uBands;
		Allocate();
	}

	void Allocate() {
		mDBand = new double[mSize];

		if (mNumLBands > 0) {
			mLBand = new double[mNumLBands][];
		}

		if (mNumUBands > 0) {
			mUBand = new double[mNumUBands][];
		}

		int i;
		for (i = 0; i < mNumLBands; ++i) {
			int lowerSize = mSize - 1 - i;
			mLBand[i] = new double[lowerSize];
		}

		for (i = 0; i < mNumUBands; ++i) {
			int upperSize = mSize - 1 - i;
			mUBand[i] = new double[upperSize];
		}
	}

	double get(int row, int column) {
		assert 0 <= row && row < mSize && 0 <= column && column < mSize : "Invalid row or column in BandedMatrix::operator\n";

		int band = column - row;
		if (band > 0) {
			if (--band < mNumUBands && row < mSize - 1 - band) {
				return mUBand[band][row];
			}
		} else if (band < 0) {
			band = -band;
			if (--band < mNumLBands && column < mSize - 1 - band) {
				return mLBand[band][column];
			}
		} else {
			return mDBand[row];
		}

		return 0.0;
	}

	public void set(int row, int column, double v) {
		assert 0 <= row && row < mSize && 0 <= column && column < mSize : "Invalid row or column in BandedMatrix::operator\n";

		int band = column - row;
		if (band > 0) {
			if (--band < mNumUBands && row < mSize - 1 - band) {
				mUBand[band][row] = v;
			}
		} else if (band < 0) {
			band = -band;
			if (--band < mNumLBands && column < mSize - 1 - band) {
				mLBand[band][column] = v;
			}
		} else {
			mDBand[row] = v;
		}
	}

	// ----------------------------------------------------------------------------
	boolean SolveSystem(double[] bVector) {
		return CholeskyFactor() && SolveLower(bVector) && SolveUpper(bVector);
	}

	// ----------------------------------------------------------------------------
	boolean SolveSystem(double[][] bMatrix, int numBColumns) {
		return CholeskyFactor() && SolveLower(bMatrix, numBColumns) && SolveUpper(bMatrix, numBColumns);
	}

	// ----------------------------------------------------------------------------
	boolean SolveLower(double[] dataVector) {
		for (int row = 0; row < mSize; ++row) {
			double lowerRR = this.get(row, row);
			if (Math.abs(lowerRR) < this.ZERO_TOLERANCE) {
				return false;
			}

			for (int column = 0; column < row; ++column) {
				double lowerRC = this.get(row, column);
				dataVector[row] -= lowerRC * dataVector[column];
			}

			dataVector[row] /= lowerRR;
		}

		return true;
	}

	// ----------------------------------------------------------------------------
	boolean SolveUpper(double[] dataVector) {
		for (int row = mSize - 1; row >= 0; --row) {
			double upperRR = this.get(row, row);
			if (Math.abs(upperRR) < this.ZERO_TOLERANCE) {
				return false;
			}

			for (int column = row + 1; column < mSize; ++column) {
				double upperRC = this.get(row, column);
				dataVector[row] -= upperRC * dataVector[column];
			}

			dataVector[row] /= upperRR;
		}

		return true;
	}

	// ----------------------------------------------------------------------------
	boolean SolveLower(double[][] dataMatrix, int numColumns) {
		for (int row = 0; row < mSize; ++row) {
			double lowerRR = this.get(row, row);
			if (Math.abs(lowerRR) < this.ZERO_TOLERANCE) {
				return false;
			}

			int bCol;
			for (int column = 0; column < row; ++column) {
				double lowerRC = this.get(row, column);
				for (bCol = 0; bCol < numColumns; ++bCol) {
					dataMatrix[row][bCol] -= lowerRC * dataMatrix[column][bCol];
				}
			}

			double inverse = 1.0 / lowerRR;
			for (bCol = 0; bCol < numColumns; ++bCol) {
				dataMatrix[row][bCol] *= inverse;
			}
		}

		return true;
	}

	// ----------------------------------------------------------------------------
	boolean SolveUpper(double[][] dataMatrix, int numColumns) {
		for (int row = mSize - 1; row >= 0; --row) {
			double upperRR = this.get(row, row);
			if (Math.abs(upperRR) < this.ZERO_TOLERANCE) {
				return false;
			}

			int bCol;
			for (int column = row + 1; column < mSize; ++column) {
				double upperRC = this.get(row, column);
				for (bCol = 0; bCol < numColumns; ++bCol) {
					dataMatrix[row][bCol] -= upperRC * dataMatrix[column][bCol];
				}
			}

			double inverse = 1.0 / upperRR;
			for (bCol = 0; bCol < numColumns; ++bCol) {
				dataMatrix[row][bCol] *= inverse;
			}
		}

		return true;
	}

	// ----------------------------------------------------------------------------
	boolean CholeskyFactor() {
		assert mNumLBands == mNumUBands : "Bands must be equal in CholeskyFactor";

		if (mNumLBands != mNumUBands) {
			return false;
		}

		int sizeM1 = mSize - 1;

		int k, kMax;
		for (int i = 0; i < mSize; ++i) {
			int jMin = i - mNumLBands;
			if (jMin < 0) {
				jMin = 0;
			}

			int j;
			for (j = jMin; j < i; ++j) {
				kMax = j + mNumLBands;
				if (kMax > sizeM1) {
					kMax = sizeM1;
				}

				for (k = i; k <= kMax; ++k) {
					double v = this.get(k, i) - (this.get(i, j) * this.get(k, j));
					this.set(k, i, v);
				}
			}

			kMax = j + mNumLBands;
			if (kMax > sizeM1) {
				kMax = sizeM1;
			}

			for (k = 0; k < i; ++k) {
				this.set(k, i, this.get(i, k));
			}

			double diagonal = this.get(i, i);
			if (diagonal <= 0.0) {
				return false;
			}
			double invSqrt = BMath.InvSqrt(diagonal);
			for (k = i; k <= kMax; ++k) {
				this.set(k, i, this.get(k, i) * invSqrt);
			}
		}

		return true;
	}
}
